#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/time.h>
#include <time.h>
#include <assert.h>

#include "PQ.h"
#include "STScheduler.h"


typedef struct Task_t
{
    struct timeval  nextRun     ;
    int (*func)(void*)          ;
    void*           data        ;
    suseconds_t     period      ;               /*  The period added to current time for the next operation in microsec     */
}Task;


struct Scheduler_t
{
    PQ* readyQ;                                 /*  Priority Queue                                                          */
};

/*  Compare function between 2 tasks    */
static int CompareFunc(void* _task1, void* _task2)
{
    Task* a = (Task*)_task1;
    Task* b = (Task*)_task2;

    return timercmp(&(a->nextRun), &(b->nextRun), <); /*  return value bigger than 0 if a < b, else 0 */
}


Scheduler* SchedCreate(int _size)
{
    Scheduler* sc = 0;

    assert(_size);

    sc = (Scheduler*)malloc(sizeof(Scheduler));
    if(!sc)
    {
        perror("scheduler allocation");
        return 0;
    }
    sc->readyQ = PQCreate(_size, CompareFunc);

    return sc;
}


int SchedRunEvery(Scheduler* _sc, int (*_func)(void*) , int _period, void* _data)
{
    Task* ts = 0;

    assert(_sc);
    assert(_func);

    ts = (Task*)malloc(sizeof(Task));
    if(!ts)
    {
        perror("error on task allocation");
        return -1;
    }

    gettimeofday(&(ts->nextRun), NULL);                     /*  Get the current time                        */
    ts->period  = _period;                                  /*  Convert period from millisec to microsec    */
    ts->func    = _func;
    ts->data    = _data;
    (ts->nextRun).tv_sec  += ts->period/1000;               /*  from msec -> sec.                           */
    (ts->nextRun).tv_usec += (ts->period%1000)*1000;
    PQPushBack(_sc->readyQ, ts);                            /*  Push task to readyQ                         */

    return 0;
}


struct timeval SchedGetTimeOut(Scheduler* _scheduler)
{
    struct timeval      tv      ;              /*  struct timeval contains seconds(from 1970) and usec of second   */
    struct timeval      res     ;
    Task*               ts  = 0 ;

    assert(_scheduler);

    ts = (Task*)PQTop(_scheduler->readyQ);
    if(!ts)                                     /*  If no tasks in PQ                                               */
    {
        res.tv_sec = -1;                        /*  reutrn seconds == -1                                            */
        return res;
    }

    gettimeofday(&tv, NULL);
    timersub(&ts->nextRun, &tv, &res);          /*  subtracts current time from nextRun, the result in res          */
    if(res.tv_sec < 0)                          /*  If nextRun before the current time => timeout == 0              */
    {
        res.tv_sec  = 0;
        res.tv_usec = 0;
    }
    return res;
}


int SchedRunOnce(Scheduler* _scheduler)
{
    Task*   ts     = 0  ;
    int     funRes      ;

    assert(_scheduler);

    if(PQPop(_scheduler->readyQ, (void**)&ts))
    {
        fprintf(stderr,"empty readyQ\n");
        return -1;
    }
    funRes = (ts->func)(ts->data);                      /*  Activating the task function    */
    gettimeofday(&(ts->nextRun), NULL);
    (ts->nextRun).tv_sec  += ts->period/1000;           /*  from msec -> sec.               */
    (ts->nextRun).tv_usec += (ts->period%1000)*1000;
    if(funRes >= 0)
    {
        PQPushBack(_scheduler->readyQ,ts);              /*  PushBack to priority Q          */
    }

    return 0;
}


int SchedStart(Scheduler* _scheduler)
{
    struct timeval      tv;                             /*  struct timeval contains seconds(from 1970) and usec of second   */
    suseconds_t         res;

    assert(_scheduler);

    while(1)
    {
        tv = SchedGetTimeOut(_scheduler);
        if(tv.tv_sec == -1)
        {
            fprintf(stderr, "error: no tasks to run\n");
            return -1;
        }

        res = tv.tv_sec*1000000 + tv.tv_usec;
        if(res >0)                                      /*  if Current time is before the next time of operation => sleep   */
        {
            usleep(res);                                /*  usleep in microseconds                                          */
        }
        SchedRunOnce(_scheduler);
    }

    return 0;
}

